#include "box2rgb.h"

Box2RGB::Box2RGB()
{
    getParam();
    cloud_.reset(new pcl::PointCloud<PointT>);
    cloud_voxel_.reset(new pcl::PointCloud<PointT>);

    pub_boxes_ = n_.advertise<detect_msgs::Boxes>(topic_boxes_new_, 10);
    pub_cloud_boxes_ = n_.advertise<sensor_msgs::PointCloud2>(topic_cloud_boxes_, 10);
    pub_img_ = n_.advertise<sensor_msgs::Image>(topic_img_new_, 10);

    sub_cloud_.subscribe(n_, topic_cloud_, 10);
    sub_boxes_.subscribe(n_, topic_boxes_, 10);
    sub_img_.subscribe(n_, topic_img_, 10);

    sync_ = std::make_shared<Sync>(MySyncPolicy(10), sub_cloud_, sub_img_, sub_boxes_);
    sync_->registerCallback(boost::bind(&Box2RGB::callBack, this, _1, _2, _3));
    
}

 void Box2RGB::callBack(const sensor_msgs::PointCloud2ConstPtr& msg_cloud,
                        const sensor_msgs::ImageConstPtr& msg_img, 
                        const detect_msgs::BoxesConstPtr& msg_boxes)
 {  
    src_ = cv_bridge::toCvShare(msg_img, "bgr8")->image;
    auto t1 = std::chrono::high_resolution_clock::now();
    cv::Mat img_det = src_.clone();
    
    if(is_filter_)
    {
        pcl::fromROSMsg(*msg_cloud, *cloud_);  // 雷达坐标系,x为深度
        filterVoxel(cloud_, cloud_voxel_, filter_size_);
    }
    else
    {
        pcl::fromROSMsg(*msg_cloud, *cloud_voxel_);
    }

    pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT>);
    // pcl::PointCloud<PointT>::Ptr cloud_uv(new pcl::PointCloud<PointT>);

    pcl::transformPointCloud(*cloud_voxel_, *cloud_cam, Eigen::Affine3d(Tcl_)); // 点云转到相机坐标下
    // pcl::transformPointCloud(*cloud_cam, *cloud_uv, Eigen::Affine3d(K_));   // 像素坐标但未归一化
    
    Eigen::Vector3d p_uv(0,0,1);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_uvZ(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
    
    pcl::PointXYZRGB p;

    for(auto point : cloud_cam->points)
    {
        if(point.z < 0 || point.z > 50.0)   // 只处理激光坐标系下x正方向的值, 向前
            continue;

        p_uv[0] = point.x * fx_ / point.z + cx_;     // 像素单位,显示到rviz上会出问题
        p_uv[1] = point.y * fy_ / point.z + cy_;     // 像素单位,显示到rviz上会出问题

        if(p_uv[0] >= 0 && p_uv[0] <= src_.cols &&
        p_uv[1] >= 0 && p_uv[1] <= src_.rows)
        {   
            p.x = (p_uv[0]);
            p.y = (p_uv[1]);
            // p.x = cloud_cam->points[i].x;
            // p.y = cloud_cam->points[i].y;
            p.z = point.z;
            // p.b = src_.at<cv::Vec3b>(int(p_uv[1]), int(p_uv[0]))[0];
            // p.g = src_.at<cv::Vec3b>(int(p_uv[1]), int(p_uv[0]))[1];
            // p.r = src_.at<cv::Vec3b>(int(p_uv[1]), int(p_uv[0]))[2];
            // std::cout << std::fixed << std::setprecision(4) << " point:" << point.x << " " << point.y << std::endl;

            p.b = src_.ptr<uchar>(int(p_uv[1]))[int(p_uv[0]) * 3];
            p.g = src_.ptr<uchar>(int(p_uv[1]))[int(p_uv[0]) * 3 + 1];
            p.r = src_.ptr<uchar>(int(p_uv[1]))[int(p_uv[0]) * 3 + 2];

            cloud_uvZ->points.emplace_back(p); 
           
        }
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZRGB>);
    
    msg_boxes_.Boxes.clear();
    cloud_rgb->points.clear();
    cloud1->points.clear();
    if(msg_boxes->Boxes.size())
    {   
        for (auto it : msg_boxes->Boxes)   // 框信息
        {   
            
            // double bbox[4] = {it.x, it.y, it.w, it.h};
            // cv::Rect box = get_rect(src_, bbox);  // 这里需要还原尺度
            cv::Rect box = cv::Rect(it.x, it.y, it.w, it.h); 
            cloud1->points.clear();
            pcl::copyPointCloud(*cloud_uvZ, *cloud1);
         
           
            passThroughFilter(cloud1, box);    // 通过uv过滤xy
            
            if(!cloud1->points.size())
                continue;
    
            Eigen::Vector3d cloud_min(0,0,50);
            pcl::PointXYZRGB point_1;
            
            // 处理分割出来的点云cloud2
            for (auto point : cloud1->points)
            {
                if(point.z < cloud_min[2])
                    cloud_min = Eigen::Vector3d(point.x, point.y, point.z);
                
                point.x = (point.x - cx_)  * point.z  / fx_;  // 像素还原到相机
                point.y = (point.y - cy_)  * point.z  / fy_;
                // point.z = point.z;
                // point.r = point.r;
                // point.g = point.g;
                // point.b = point.b;
                cloud_rgb->points.emplace_back(point);
            }
            
            // *cloud_rgb += *cloud1;


            float x =  (cloud_min[0] - cx_)  * cloud_min[2] / fx_;
            float y =  (cloud_min[1] - cy_)  * cloud_min[2] / fy_;

            msg_box_.class_name = it.class_name;
            msg_box_.id = it.id;
            msg_box_.score = it.score;
            msg_box_.x = box.x;
            msg_box_.y = box.y;
            msg_box_.w = box.width;
            msg_box_.h = box.height;
            msg_box_.p_z = cloud_min[2];
            msg_box_.p_x = x;
            msg_box_.p_y = y; 

            msg_boxes_.Boxes.emplace_back(msg_box_);

            std::cout << msg_box_.class_name << " " 
                << msg_box_.x << " " << msg_box_.y << " " << msg_box_.w << " " << msg_box_.h << " "
                << msg_box_.p_x << " " << msg_box_.p_y << " " << msg_box_.p_z << " "
                << msg_box_.score << std::endl;
            
            drawBoxAndDist(img_det, msg_box_);
        }
    }

    pub_boxes_.publish(msg_boxes_);  // 发布带深度的检测框

    auto t2 = std::chrono::high_resolution_clock::now();
    auto cost_time = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
    
    sensor_msgs::PointCloud2Ptr msg_cloud_boxes(new sensor_msgs::PointCloud2);
    pcl::toROSMsg(*cloud_rgb, *msg_cloud_boxes);
    msg_cloud_boxes->header.stamp = msg_cloud->header.stamp;
    msg_cloud_boxes->header.frame_id = frame_id_;
    
    // 发点云
    pub_cloud_boxes_.publish(msg_cloud_boxes);

    // 发布结果图片
    sensor_msgs::ImagePtr msg_img_new;
    msg_img_new = cv_bridge::CvImage(msg_cloud->header, "bgr8", img_det).toImageMsg();
    pub_img_.publish(msg_img_new);


    ROS_INFO("Ranging completed %.2f fps", 1000. / cost_time.count()  );

    



 }

void Box2RGB::filterVoxel(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out, float size)
{
    pcl::VoxelGrid<PointT> sor; // 创建体素滤波器
    sor.setInputCloud(cloud_in);          // 输入点云
    // sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体,m
    sor.setLeafSize(size, size, size);
    sor.filter(*cloud_out);
}

void Box2RGB::passThroughFilter( pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_in, const cv::Rect& box)
{   
    pcl::PointCloud<pcl::PointXYZRGB> cloud_x, cloud_y;
    pcl::PassThrough<pcl::PointXYZRGB> pass_x;  // 创建直通滤波器对象
    pass_x.setInputCloud(cloud_in);            // 设置输入对象,传地址
    pass_x.setFilterFieldName("x");             // 设置过滤时所需要点云类型的X字段
    pass_x.setFilterLimits(box.x, box.x + box.width);   
    pass_x.filter(*cloud_in);     
    
    pcl::PassThrough<pcl::PointXYZRGB> pass_y; 
    pass_y.setInputCloud(cloud_in);  
    pass_y.setFilterFieldName("y");             // 设置过滤时所需要点云类型的Y字段
    pass_y.setFilterLimits(box.y, box.y + box.height );   
    pass_y.filter(*cloud_in);

    // pcl::PassThrough<pcl::PointXYZRGB> pass_z; 
    // pass_z.setInputCloud(cloud_in);  
    // pass_z.setFilterFieldName("z");             // 设置过滤时所需要点云类型的Z字段
    // pass_z.setFilterLimits(0.1, 30);           // 设置过滤字段的范围.  防止0
    // pass_z.filter(*cloud_in);
}

void Box2RGB::Rt2T(Eigen::Isometry3d& T)
{
    // R,t->Tcl 
    Eigen::Vector3d R_euler(roll_, pitch_, yaw_); // 旋转
    R_euler = R_euler / 180 * M_PI;
    
    Eigen::Vector3d t(x_, y_, z_);  // 平移
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(R_euler[0], Eigen::Vector3d::UnitZ()) * 
        Eigen::AngleAxisd(R_euler[1], Eigen::Vector3d::UnitY()) * 
        Eigen::AngleAxisd(R_euler[2], Eigen::Vector3d::UnitX());
    T.rotate(R);
    T.pretranslate (t);
}

cv::Rect Box2RGB::get_rect(cv::Mat& img, double bbox[4]) {
  float l, r, t, b;
  float r_w = 640. / (img.cols * 1.0);
  float r_h = 640. / (img.rows * 1.0);
  if (r_h > r_w) 
  {
    l = bbox[0] - bbox[2] / 2.f;
    r = bbox[0] + bbox[2] / 2.f;
    t = bbox[1] - bbox[3] / 2.f - (640. - r_w * img.rows) / 2;
    b = bbox[1] + bbox[3] / 2.f - (640. - r_w * img.rows) / 2;
    l = l / r_w;
    r = r / r_w;
    t = t / r_w;
    b = b / r_w;
  } 
  else 
  {
    l = bbox[0] - bbox[2] / 2.f - (640. - r_h * img.cols) / 2;
    r = bbox[0] + bbox[2] / 2.f - (640. - r_h * img.cols) / 2;
    t = bbox[1] - bbox[3] / 2.f;
    b = bbox[1] + bbox[3] / 2.f;
    l = l / r_h;
    r = r / r_h;
    t = t / r_h;
    b = b / r_h;
  }
  return cv::Rect(round(l), round(t), round(r - l), round(b - t));
}

void Box2RGB::drawBoxAndDist(cv::Mat& img, const detect_msgs::Box& res)
{
    // double bbox[4] = {res.x*1.0, res.y*1.0, res.w*1.0, res.h*1.0};
    // cv::Rect box = get_rect(img, bbox);  // 这里需要还原尺度

    cv::Rect box = cv::Rect(res.x, res.y, res.w, res.h); 

    cv::rectangle(img, box, cv::Scalar(0, 0, 255), 2);

    std::stringstream ss;
    ss << std::fixed << std::setprecision(2) << res.score;
    std::string s = res.class_name + " " + ss.str();

    // cv::putText(img,  s, cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);

    auto font_face = cv::FONT_HERSHEY_DUPLEX;
    auto font_scale = 0.8;
    int thickness = 1;
    int baseline=0;
    auto s_size = cv::getTextSize(s, font_face, font_scale, thickness, &baseline);

    if(box.tl().y - s_size.height < 0)
    {
        cv::rectangle(img,
            cv::Point(box.tl().x, box.tl().y),
            cv::Point(box.tl().x + s_size.width, box.tl().y + s_size.height+7),
            cv::Scalar(0, 0, 255), -1);
        cv::putText(img, s, cv::Point(box.tl().x, box.tl().y + s_size.height),
                font_face , font_scale, cv::Scalar(255, 255, 255), thickness);

    }
    else
    {
          cv::rectangle(img,
              cv::Point(box.tl().x, box.tl().y - s_size.height - 7),
              cv::Point(box.tl().x + s_size.width, box.tl().y),
              cv::Scalar(0, 0, 255), -1);
        cv::putText(img, s, cv::Point(box.tl().x, box.tl().y - 5),
                  font_face , font_scale, cv::Scalar(255, 255, 255), thickness);
    }
    
    cv::circle(img, cv::Point(box.x + box.width / 2, box.y + box.height / 2), 4, cv::Scalar(0, 0, 255), -1);
    

    double z = sqrt( res.p_x * res.p_x + res.p_y * res.p_y + res.p_z * res.p_z );
    
    std::stringstream s1;
    s1 << std::fixed << std::setprecision(2) << z;
    s = " Dist: " + s1.str();

    cv::putText(img, s, cv::Point(box.x + box.width / 2, box.y + box.height / 2),
                font_face , font_scale, cv::Scalar(0, 0, 255), thickness);

}

void Box2RGB::getParam()
{
    n_.getParam("topic_boxes", topic_boxes_);
    n_.param<std::string>("topic_boxes", topic_boxes_, "/detect/boxes");
    
    n_.getParam("topic_cloud", topic_cloud_);
    n_.param<std::string>("topic_cloud", topic_cloud_, "/velodyne_points");

    n_.getParam("topic_img", topic_img_);
    n_.param<std::string>("topic_img", topic_img_, "/image_left");

    n_.getParam("topic_cloud_boxes", topic_cloud_boxes_);
    n_.param<std::string>("topic_cloud_boxes", topic_cloud_boxes_, "/detect/cloud_boxes");

    n_.getParam("topic_boxes_new", topic_boxes_new_);
    n_.param<std::string>("topic_boxes_new", topic_boxes_new_, "/detect/boxes_new");

    n_.getParam("topic_img_new", topic_img_new_);
    n_.param<std::string>("topic_img_new", topic_img_new_, "/detect/image_with_dist");

    
    n_.getParam("frame_id", frame_id_);
    n_.param<std::string>("frame_id", frame_id_, "map");

    n_.getParam("filter_size", filter_size_);
    n_.param<float>("filter_size",filter_size_, 0.1);

     n_.getParam("is_filter", is_filter_);
    n_.param<bool>("is_filter", is_filter_, false);

    // -------------内参-----------------------
    n_.getParam("fx", fx_);
    n_.param<float>("fx", fx_, 718.856); 
    n_.getParam("fy", fy_);
    n_.param<float>("fy", fy_, 718.856); 
    n_.getParam("cx", cx_);
    n_.param<float>("cx", cx_, 607.1928); 
    n_.getParam("cy", cy_);
    n_.param<float>("cy", cy_, 185.2157); 
    
    n_.getParam("k1", k1_);
    n_.param<float>("k1", k1_, 0.0); 
    n_.getParam("k2", k2_);
    n_.param<float>("k2", k2_, 0.0); 
    n_.getParam("k3", k3_);
    n_.param<float>("k3", k3_, 0.0); 
    
    n_.getParam("p1", p1_);
    n_.param<float>("p1", p1_, 0.0); 
    n_.getParam("p2", p2_);
    n_.param<float>("p2", p2_, 0.0); 

    //  --------Tcl--------
    n_.getParam("roll", roll_);
    n_.param<float>("roll", roll_, 90.); 
    n_.getParam("pitch_", pitch_);
    n_.param<float>("pitch", pitch_, -90.); 
    n_.getParam("yaw", yaw_);
    n_.param<float>("yaw", yaw_, 0.0); 
    
    n_.getParam("x", x_);
    n_.getParam("y", y_);
    n_.getParam("z", z_);

    n_.param<float>("x", x_, -1.198459927713e-02);  
    n_.param<float>("y", y_, 5.403984729748e-02); 
    n_.param<float>("z", z_, -2.921968648686e-01); 

    Rt2T(Tcl_);
    std::cout << "\n --T_lidar2cam:\n" << Tcl_.matrix() << std::endl;
    std::cout << "\n-------------------\n" << std::endl;
    
    std::cout << " --filter_size : "<< filter_size_ << std::endl;
    std::cout << " --is_filter: " << is_filter_ << std::endl; 
    std::cout << " --frame_id: " << frame_id_ << std::endl; 

    std::cout << " --topic_cloud: " << topic_cloud_ << std::endl; 
    std::cout << " --topic_boxes: " << topic_boxes_ << std::endl; 
    std::cout << " --topic_img: " << topic_img_ << std::endl; 
    
    std::cout << " --topic_cloud_new: " << topic_cloud_boxes_ << std::endl; 
    std::cout << " --topic_boxes_new: " << topic_boxes_new_ << std::endl; 
    std::cout << " --topic_img_new: " << topic_img_new_ << std::endl; 

    std::cout << "\n---------------\n" << std::endl;

}
